#ifndef PTZ_UART_HEADER_FILE
#define PTZ_UART_HEADER_FILE

#include <arpa/inet.h>
#include <curl/curl.h>
#include <errno.h>
#include <fcntl.h>
#include <netinet/in.h>
#include <stdarg.h>
#include <sys/socket.h>
#include <sys/time.h>

#include <atomic>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <fstream>
#include <iostream>
#include <limits>
#include <map>

#include "global.h"
#include "nlohmann/json.hpp"
#include "serialPort.h"

// 爆闪灯   辅助开关1     注：三开
// 喇叭     辅助开关2
// 天线     辅助开关3

#define PI 3.14159

extern uint16_t zoom_num[25];
extern float hfov_arr[26];
extern float vfov_arr[26];

// PID 控制器类，实现了经典的 PID 控制算法
class PIDController {
    private:
        double kp;  // 比例系数，用于根据当前误差调整控制输出
        double ki;  // 积分系数，用于消除系统的稳态误差
        double kd;  // 微分系数，用于预测误差的变化趋势，改善系统的动态响应
        double prev_error;  // 上一次计算时的误差，用于计算误差的微分
        double integral;    // 误差的积分值，不断累积误差
        double integral_limit; // 积分限幅，防止积分项累积过大导致积分饱和


        // 误差归一化函数
        double normalizeError(double error) {
            if (error > 180) {
                error -= 360;
            } else if (error < -180) {
                error += 360;
            }
            return error;
        }

        

    public:
        // 构造函数，用于初始化 PID 控制器的参数
        // p: 比例系数
        // i: 积分系数
        // d: 微分系数
        // limit: 积分限幅
        PIDController(double p, double i, double d, double limit) 
        : kp(p), ki(i), kd(d), prev_error(0), integral(0)
        , integral_limit(limit) 
        {}
    
        // 计算控制输出的函数
        // setpoint: 设定的目标值
        // current_value: 当前系统的实际值
        double calculate(double setpoint, double current_value, int current_zoom) {

            // 计算当前误差，即目标值与实际值的差值
            double error = normalizeError(setpoint - current_value);
    
            // 对误差进行积分，不断累加误差值
            integral += error;
    
            // 积分限幅，防止积分项累积过大导致积分饱和
            if (integral > integral_limit) {
                integral = integral_limit;
            } else if (integral < -integral_limit) {
                integral = -integral_limit;
            }
    
            // 计算误差的微分，即当前误差与上一次误差的差值
            double derivative = error - prev_error;


            // 根据 PID 算法的公式计算控制输出
            // 控制输出 = 比例项 + 积分项 + 微分项
            double output = kp * error + ki * integral + kd * derivative;
            // 
    
            // 更新上一次的误差值，为下一次计算微分做准备
            prev_error = error;
            double damping = 0.8 + (static_cast<double>(current_zoom) - 1) / (25 - 1) * 0.4;
            output = output * damping;

            // output = output > 60 ? 60 : output;

            return output;
        }

        void setPID(int p, int i, int d)
        {
            kp = p;
            ki = i;
            kd = d;
        }

        // 重置积分项的函数，可在需要时将积分项清零
        void resetIntegral() { integral = 0; }
};

class GimbalClient
{
   public:
    GimbalClient();
    ~GimbalClient();
    serialPort ptserial;

   private:
    int screen_show_fd;
    struct sockaddr_in remote_addr;
    struct sockaddr_in local_addr;

#ifdef RS485
    const char *ptserial_fd = "/dev/ttyAMA1";
#else
    const char *ptserial_fd = "/dev/ttyAMA4";
#endif

    // 初始化方位角 PID 控制器，设置比例系数为 2.0，积分系数为 0.1，微分系数为 0.01，积分限幅为 100.0
    // 新的 PID 参数，用于初始转向
    PIDController tilt_pid_initial = PIDController(2.0, 0.05, 0.01, 50.0);
    PIDController pan_pid_initial = PIDController(2.0, 0.05, 0.01, 50.0);

    PIDController tilt_pid = PIDController(3.0, 0.05, 0.5, 50.0); 
    PIDController pan_pid = PIDController(3.0, 0.05, 2.0, 50.0);
        
    double current_pan;
    double current_tilt;
    int current_zoom;

    std::atomic<bool> gimbal_update{false}; // 云台固件升级标志位
    uint8_t counter_action_ = 0; // 反制
        
    void lpp_command(double pan_control, double tilt_control);
public:

    PIDController& get_pan_pid() {
        return pan_pid;
    }

    PIDController& get_tilt_pid() {
        return tilt_pid;
    }

   public:
    bool gimbal_init();

    bool set_pan_absolute(float angle);
    bool set_tilt_absolute(float angle);

    bool set_pan_absolute(float angle, uint8_t speed);
    bool set_tilt_absolute(float angle, uint8_t speed);

    bool set_pan_relative(float angle, uint8_t speed);
    bool set_tilt_relative(float angle, uint8_t speed);

    bool set_pan_relative(float angle);
    bool set_tilt_reltive(float angle);

    void lpp_control_step(double target_pan, double target_tilt, double error_prescision = 1.0, int current_zoom = 1,bool use_initial_pid = false);
    // void lpp_control_step_pixel(double cx, double cy);
    void lpp_control_step_pixel(bbox_t phase_bbox,int& current_zoom);
    bool lpp_control_to_destination(double target_pan, double target_tilt, int zoom, int step_count = 100, double error_prescision = 1.0);

    bool set_zoom_ratio(uint8_t ratio);
    bool add_zoom_ratio();
    bool sub_zoom_ratio();

    bool set_countermeasure(bool open_or_close);

    uint8_t get_current_zoom();
    float get_current_pan();
    float get_current_tilt();

    void start_thread();
    bool update_pt_absolute();

    bool stop_move();
    bool move_up(uint8_t speed);
    bool move_left(uint8_t speed);
    bool move_right(uint8_t speed);
    bool move_down(uint8_t speed);

    bool move_left_top(uint8_t speed);
    bool move_right_top(uint8_t speed);
    bool move_left_down(uint8_t speed);
    bool move_right_down(uint8_t speed);

    bool set_preset_point(uint8_t point);
    bool move_to_preset_point(uint8_t point);

    bool send_target_to_box(uint8_t event_id, std::string alarm_type, uint16_t target_w,
                            uint16_t target_h, uint8_t alarmlv, uint8_t block_class);

    bool update_screen_osd(uint8_t type, uint8_t wp = 0);

    bool set_counter_action(uint8_t counter_action, bool state);
    bool requset_record();
    bool ptz_update(char *update_path);
    bool ptz_update_485(char *update_path);

    int findClosestIndex(const float nums[], int size,
                         float target);  // 查询最近视场角索引

    //============================开关设备============================

    //============================IR设备============================
    bool IR_switch(bool state);
    bool setIRlm(uint8_t lm);
    bool setIRzoom(bool state, uint8_t ratio);
};

#endif